#pragma once

#include "vio/front_end/feature.h"
#include "vio/front_end/feature_detection.h"
#include "vio/front_end/frame.h"
#include "vio/tools/homography.h"
#include "vio/tools/math_utils.h"
#include "vio/vio_common.h"

namespace vio {

class FrameHandlerMono;

// bootstrapping the map from the first two views
namespace initialization {

enum InitResult { FAILURE, NO_KEYFRAME, SUCCESS };

// tracks features using Lucas-Kanade tracker and then estimates a homography
class KltHomographyInit {
  friend class FrameHandlerMono;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  typedef std::shared_ptr<KltHomographyInit> Ptr;

  Frame::Ptr frame_ref_;
  KltHomographyInit() {}
  InitResult AddFirstFrame(Frame::Ptr frame_ref);
  InitResult AddSecondFrame(Frame::Ptr frame_ref);
  void Reset();

 protected:
  std::vector<cv::Point2f>
      px_ref_;  // keypoints to be tracked in reference frame
  std::vector<cv::Point2f> px_cur_;     // tracked keypoints in current frame.
  std::vector<Eigen::Vector3d> f_ref_;  // bearing vectors corresponding to the
                                        // keyponts in the reference image.
  std::vector<Eigen::Vector3d> f_cur_;  // bearing vectors corresponding to the
                                        // keypoints in the current image.
  std::vector<double>
      disparities_;           // disparity between first and second frame.
  std::vector<int> inliers_;  // inliers after the geometric check (homography)
  std::vector<Eigen::Vector3d>
      xyz_in_cur_;  // 3D points computed during the geometric check.
  Sophus::SE3d
      T_cur_from_ref_;  // computed transformation between the first two frames.
};

// detect fast corners in the image.
void DetectFeatures(Frame::Ptr frame,
                    std::vector<cv::Point2f>& px_vec,
                    std::vector<Eigen::Vector3d>& f_vec);

// compute optical flow (Lucas Kanade) for selected keypoints.
void TrackKLT(Frame::Ptr frame_ref,
              Frame::Ptr frame_cur,
              std::vector<cv::Point2f>& px_ref,
              std::vector<cv::Point2f>& px_cur,
              std::vector<Eigen::Vector3d>& f_ref,
              std::vector<Eigen::Vector3d>& f_cur,
              std::vector<double>& disparities);

void ComputeHomography(const std::vector<Eigen::Vector3d>& f_ref,
                       const std::vector<Eigen::Vector3d>& f_cur,
                       double focal_length,
                       double reprojection_threshold,
                       std::vector<int>& inliers,
                       std::vector<Eigen::Vector3d>& xyz_in_cur,
                       Sophus::SE3d& T_cur_from_ref);

}  // namespace initialization
}  // namespace vio